/*
 Example sketch for the PS3 Bluetooth library - developed by Kristian Lauszus
 For more information visit my blog: http://blog.tkjelectronics.dk/ or
 send me an e-mail:  kristianl@tkjelectronics.com
 */

#include <PS3BT.h>
#include <usbhub.h>

// Satisfy the IDE, which needs to see the include statment in the ino too.
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif
#include <SPI.h>
#include <AFMotor.h>
#include <Servo.h> 

Servo servo1;
AF_DCMotor motor(1);

USB Usb;
//USBHub Hub1(&Usb); // Some dongles have a hub inside

BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
/* You can create the instance of the class in two ways */
//PS3BT PS3(&Btd); // This will just create the instance
PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch

bool printTemperature, printAngle;
int pos = 0; 
void setup() {
  Serial.begin(9600);
#if !defined(__MIPSEL__)
  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
#endif
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while (1); //halt
  }
  Serial.print(F("\r\nPS3 Bluetooth Library Started"));
  motor.setSpeed(255);
  motor.run(RELEASE);
  servo1.attach(9);
  servo1.write(90); 
}

void faw(){
  motor.run(BACKWARD);
}

void back(){
  motor.run(FORWARD);
}
void stop(){
  motor.run(RELEASE);
}
void right(){
  servo1.write(120); 
}
void left(){
  servo1.write(60); 
}
void front(){
  servo1.write(90); 
}
void loop() {
  Usb.Task();

  if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
      if(PS3.getAnalogHat(LeftHatY) < 90){
         Serial.print(F("\r\nUp :"));
         Serial.print(PS3.getAnalogHat(LeftHatY));
         faw();
      }else if(PS3.getAnalogHat(LeftHatY) > 180){
         Serial.print(F("\r\nDown :"));
         Serial.print(PS3.getAnalogHat(LeftHatY));
         back();
      }
      if(PS3.getAnalogHat(LeftHatX) > 180){
         Serial.print(F("\r\nRight :"));
         Serial.print(PS3.getAnalogHat(LeftHatX));
         right();
      }else if(PS3.getAnalogHat(LeftHatX) < 90){
         Serial.print(F("\r\nLeft :"));
         Serial.print(PS3.getAnalogHat(LeftHatX));
         left();
      }
       if(PS3.getAnalogHat(LeftHatY)>90 && PS3.getAnalogHat(LeftHatY) <180){
         Serial.print(F("\r\n stop :"));
         Serial.println(PS3.getAnalogHat(RightHatY)+" "+PS3.getAnalogHat(LeftHatX));
         stop();
      }
      if(PS3.getAnalogHat(LeftHatX) >90 && PS3.getAnalogHat(LeftHatX) <180 ){
         Serial.print(F("\r\n front :"));
         front();
      }
      if (PS3.getButtonClick(CIRCLE)) {
        Serial.print(F("\r\nCircle"));
        stop();
      }
      if (PS3.getButtonClick(CROSS)) {
         stop();
      }

      if (PS3.getButtonClick(UP)) {
        faw();
      } if (PS3.getButtonClick(RIGHT)) {
        right();
      } if (PS3.getButtonClick(DOWN)) {
        back();
      } if (PS3.getButtonClick(LEFT)) {
        left();
      }
    }
  }
